#include "headfile.h"
#include "pid.h"
float PID_Release(PID_Struct *PID, float target, float now){
    float err,err_dt,result;

    err = target - now;
    err_dt = err - PID->last_err;

    err_dt *= 0.384f;
    err_dt += PID->last_d * 0.615f;  //低通滤波

    PID->last_err = err;
    PID->last_d = err_dt;

    PID->i += err * PID->I_TIME;

    Limit(PID->i, PID->i_max);
    

    result = err * PID->KP + err_dt * PID->KD + PID->i * PID->KI;
    return result;
}

void reset_PID(PID_Struct *PID){
    PID->i = 0;
    PID->last_err = 0;
    PID->last_d = 0;
}

void set_PID(PID_Struct *PID,float p,float i,float d){
    PID->KP=p;
    PID->KI=i;
    PID->KD=d;
}
